function [ isNP, confidence ] = isNextPoint( nextPoint, points, maxDistToNext, dt)
%ISNEXTWAYPOINT tests if the given waypoint could be the next in series
%confidence is a relative value, just compareable for the same point
%sequence
distance = norm(nextPoint - points(end,:));
if(size(points,1) > 1)
    computedNextPoint = extrapolate(points, dt);
    error =  norm(computedNextPoint-nextPoint);
    marginRadius = norm(computedNextPoint - points(end,:));
    %plot([computedNextWaypoint(1);computedNextWaypoint(1)-marginRadius], [computedNextWaypoint(2);computedNextWaypoint(2)], 'c');
    if(error > marginRadius && distance > maxDistToNext)
        isNP = false;
        confidence = error;
    else
        isNP = true;
        confidence = 1/(error+1);
    end
else
    if(distance > maxDistToNext)
        isNP = false;
        confidence = distance;
    else
        isNP = true;
        confidence = 1/(distance+1);
    end
end

